/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "CScanMeasurementSet.h"




CScanMeasurementSet::CScanMeasurementSet(const char *logfilename) : 
numMeas(0)
{
	lx=0,ly=0,lz=0,la=0; ///< laser position
	numMeas = 0;
	readScanFile(logfilename);
	
	
}


CScanMeasurementSet::CScanMeasurementSet(const char *positionfile, const char *rangingfile){
	lx=0,ly=0,lz=0,la=0; ///< laser position
	numMeas = 0;
	readMaciDataFiles(positionfile, rangingfile);
}


CScanMeasurementSet::~CScanMeasurementSet(){
}


int CScanMeasurementSet::forwardString(const char *buf, int begin,int items){
	int i=begin+1;
	int cnt = 0;
	while(cnt<items){
		if(buf[i]==' ') cnt++;
		i++;
	}
	return i;
}

/// For reading the data from file
int CScanMeasurementSet::readScanFile(const char *fname){
		float time_s;
		int i=0;
		char in_line[16096];
		char *ret;
		char c=0;
		
		vector<float> r;
		vector<float> phii;
		float x,y,a;
		
		FILE *f = fopen(fname,"rt");
		if(f == NULL){
			fprintf(stderr,"Error opening File '%s'",fname);
			exit(1);
		}
		
		ret = fgets (in_line, 16096, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return 0;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return 1;
				}
		}
		int cnt=0;
		while( (c!='\n')){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		/// THIS IS THE FIRST SCAN
		Grid::pose p;
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		numMeas = (cnt-3)/2;
		i =0;
		poses.push_back(p);
		
		while(i<((cnt-3)/2)){
			float range, ang;
			sscanf((in_line+pointer_ind),"%f %f",&range,&ang);
			pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
			i++;
			r.push_back(range);
			angles.push_back(ang);
		}
		ranges.push_back(r);
		
		/// ALL THE REST
		while(!feof(f)){
			r.clear();
			ret = fgets (in_line, 16096, f );
			if(ret==NULL){
				if(feof(f)){
					fprintf(stderr,"Reached end of file -- Alles klar!\n");
					return 0;
				}else{
					fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
					return 1;
				}
			}
			
			sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
			int pointer_ind=0;
			pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
			poses.push_back(p);
			i =0;
			while(i<((cnt-3)/2)){
				float range, ang;
				sscanf((in_line+pointer_ind),"%f %f",&range,&ang);
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
				r.push_back(range);
			}
			ranges.push_back(r);
		}
		
		if(ranges.size()==0){
			dPrint(1,"Invalid data size!!!");
			return -1;
		}
		if(angles.size()!=ranges[0].size()){
			dPrint(1,"Invalid data size!!!");
			return -1;
		}
		if(poses.size() != ranges.size()){
			dPrint(1,"Invalid data size!!!");
			return -1;
		}
		return 0;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
/// MACI DATA READERS
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
bool CScanMeasurementSet::readMaciDataFiles(const char *positionFile, const char *rangingFile){
	 CPositionFileReader preader;
	 CRangingFileReader rreader;
	 
	 preader.ReadFile(positionFile);
	 rreader.ReadFile(rangingFile);
	 
  if(rreader.GetDeviceLimits(deviceLimits)){
    dPrint(1,"Got device limits min: %f, max: %f",deviceLimits.min_distance,deviceLimits.max_distance);
  }else{
    dPrint(1,"Failed to get device limits");
  }
	
  MaCI::Ranging::TDeviceInformationPosition devicePosition;
  if(rreader.GetDevicePosition(devicePosition)){
    dPrint(1,"Got device position (%f ; %f ; %f) heading (%f)",
           devicePosition.x,
           devicePosition.y,
           devicePosition.z,
           devicePosition.heading);
		lx = devicePosition.x;
    ly = devicePosition.y;
    lz = devicePosition.z;
    la = devicePosition.heading;			 	
  }else{
    dPrint(1,"Failed to get device limits");
    exit(1);
  }
	 
	///////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////
	/// MAKE SYNCHRONIZATION
	///////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////
	std::list<gim::time> times;
  if(rreader.GetTimes(times)){
    dPrint(1,"Got times... number of times %d",times.size());
  }else{
    dPrint(1,"Failed to get times ");
    exit(1);
  }
	
	MaCI::Ranging::CRangingData rdata;
	MaCI::Position::CPositionData pdata;

	std::list<gim::time>::iterator ii;
	
	for(ii=times.begin();ii!=times.end();ii++){
		  if(rreader.GetDistanceArray(rdata,*ii)){
				if(preader.getClosestTo(pdata,*ii)){
					
					const MaCI::Position::TPose2D *pose; ///< Pose
					const MaCI::Ranging::TDistance *r_meas; ///Laser Measurement
					
					pose = pdata.GetPose2D();
					if(pose == NULL){
						fprintf(stderr,"FAILED TO PARSE POSITION!!\n");
						exit(1);
					}
					Grid::pose p;
					p.x = pose->x;
					p.y = pose->y;
					p.a = pose->a;
					poses.push_back(p);
					vector<float> rang;
					rTimes.push_back(*ii);
					for(int i=0;i<rdata.GetDistanceElementCount();i++){
						r_meas = rdata.GetDistance(i);
						if(r_meas->distance > deviceLimits.min_distance && r_meas->distance < deviceLimits.max_distance){
							rang.push_back(r_meas->distance);
						}else{
							rang.push_back(0.0f);
						}
						if(ii==times.begin()) angles.push_back(r_meas->angle);
					}
					ranges.push_back(rang);
					rang.clear();
				}else{
					dPrint(1,"Failed to get Position data");
					exit(1);
				}	
			}else{
				dPrint(1,"Failed to get measurement");
			return false;
		}
	}
	
	if(ranges.size()==0){
			dPrint(1,"Invalid data size!!!");
			return false;
	}
	
	if(angles.size()!=ranges[0].size()){
		dPrint(1,"Invalid data size!!!");
		return false;
	}
	
	if(poses.size() != ranges.size()){
		dPrint(1,"Invalid data size!!!");
		return false;
	}
	
	return true;
}

/**
* Get Measurement from index ind
* @param ind index of the measurement
* @param &pos[out] position of measurement i is stored here
* @param &s[out] scan of measurement i is stored here
* @return true, if success
*/
bool CScanMeasurementSet::getMeasurement(int ind, Grid::pose &pos, Grid::TScan &s){
	if(ind < 0 || ind >= poses.size() || ind>=ranges.size()){
		return false;
	}
	if(s.N<angles.size()) s.alloc(angles.size());
	
	pos = poses[ind];
	for(int i=0;i<angles.size();i++){
		if(i>=ranges[ind].size()){
			fprintf(stderr,"ERRRR Range size = %d\n",ranges[ind].size() );
			break;
		}
		s.r[i] = ranges[ind][i];
		s.a[i] = angles[i];
	}
	s.N = ranges[ind].size();
	
	return true;
}


/**
* Get Measurement from index ind and the transformed (in 2D) to laser frame
* @param ind index of the measurement
* @param &pos[out] position of measurement i is stored here
* @param &s[out] scan of measurement i is stored here
* @return true, if success
*/
bool CScanMeasurementSet::getMeasurementInLaserFrame(int ind, Grid::pose &pos, Grid::TScan &s){
	if(ind < 0 || ind >= poses.size()){
		return false;
	}
	if(s.N<angles.size()) s.alloc(angles.size());
	
	pos = poses[ind];
	for(int i=0;i<angles.size();i++){
		s.r[i] = ranges[ind][i];
		s.a[i] = angles[i];
	}
	s.N = angles.size();
	
	Grid::pose lp; ///Laser pose		
	///Convert the position to laser frame (gives the laser pose with respect to world frame of reference)
	
	float dy =ly;
	float dx = lx;
	float alpha = atan2(dy,dx);
	float L = sqrt(dx*dx+dy*dy);
	lp.x = pos.x + L * cos(pos.a+alpha);
	lp.y = pos.y + L * sin(pos.a+alpha);
	lp.a = pos.a + la;
	//fprintf(stderr,"%.2f %.2f %.2f %d a[0]=%.2f\n", lx,ly,la,angles.size(),angles[0]);
	pos=lp;
	return true;
}
/**
* Converts a pose from robot frame to laser frame
*/
bool CScanMeasurementSet::convertToLaserFrame(Grid::pose &pos){
	Grid::pose lp; ///Laser pose		
	///Convert the position to laser frame (gives the laser pose with respect to world frame of reference)
	
	float dy =ly;
	float dx = lx;
	float alpha = atan2(dy,dx);
	float L = sqrt(dx*dx+dy*dy);
	lp.x = pos.x + L * cos(pos.a+alpha);
	lp.y = pos.y + L * sin(pos.a+alpha);
	lp.a = pos.a + la;
	//fprintf(stderr,"%.2f %.2f %.2f %d a[0]=%.2f\n", lx,ly,la,angles.size(),angles[0]);
	pos=lp;
	return true;
}




/**
* Sets a pos which is measured in laser frame to Robot frame
* 
*/
bool CScanMeasurementSet::setMeasurementToRobotFrame(int ind, Grid::pose lpos){
	if(ind < 0 || ind >= poses.size()){
		return false;
	}	
	Grid::pose lp; 		
	///Convert the position to robot frame (gives the robot pose with respect to world frame of reference)
	
	float dy = ly;
	float dx = lx;
	float alpha = atan2(dy,dx);
	float L = sqrt(dx*dx+dy*dy);
	lp.x = lpos.x - L * cos(lpos.a-la+alpha);
	lp.y = lpos.y - L * sin(lpos.a-la+alpha);
	lp.a = lpos.a - la;
	//fprintf(stderr,"%.2f %.2f %.2f %d a[0]=%.2f\n", lx,ly,la,angles.size(),angles[0]);
	poses[ind]=lp;
	return true;
}


bool CScanMeasurementSet::getPoseInLaserFrame(int ind, Grid::pose &pos){
	if(ind < 0 || ind >= poses.size()){
		return false;
	}
	pos = poses[ind];
	Grid::pose lp; ///Laser pose		
	///Convert the position to laser frame (gives the laser pose with respect to world frame of reference)
	
	float dy =ly;
	float dx = lx;
	float alpha = atan2(dy,dx);
	float L = sqrt(dx*dx+dy*dy);
	lp.x = pos.x + L * cos(pos.a+alpha);
	lp.y = pos.y + L * sin(pos.a+alpha);
	lp.a = pos.a + la;
	//fprintf(stderr,"%.2f %.2f %.2f %d a[0]=%.2f\n", lx,ly,la,angles.size(),angles[0]);
	pos=lp;
	return true;
}


/**

void writeScanLine(FILE *f,Grid::TScan &meas, float x,float y,float a){
	 ///< Pose
	int i;
	
		
*/
bool CScanMeasurementSet::saveLnMFormat(const char *fname){
	FILE *f = fopen(fname,"wt");
	int j;
	Grid::TScan meas;
	Grid::pose lp;
	
	for(int i=0;i<poses.size();i++){
		getMeasurementInLaserFrame(i, lp, meas);
		fprintf(f,"%f %f %f ",lp.x,lp.y,lp.a);
		
		for(j=0;j<angles.size()-1;j++){
			fprintf(f,"%f %f ",ranges[i][j],angles[j]);
		}
		fprintf(f,"%f %f\n",ranges[i][j],angles[j]);
	}
	
	fclose(f);
	return false;
}

bool CScanMeasurementSet::saveMaCIPositionFILE(std::string aFileName,
																								std::string aMaCIGroup,
																								std::string aInstanceName)
{
	
	if(rTimes.size()==0){
		fprintf(stderr,"Invalid timestamps - cannot save in MaCI Log format\n");
		return false;
	}
	if(rTimes.size()<poses.size()){
		fprintf(stderr,"Vector sizes do not match (rT[%d] , P[%d])! - cannot save in MaCI Log format\n",
						rTimes.size(),poses.size());
		return false;
	}
	rTimes.erase(rTimes.begin());
	CPositionFileWriter posWriter;
	posWriter.OpenFile(aFileName,aMaCIGroup,aInstanceName);
	
	
	for(int i=0;i<size();i++){
		//fprintf(stderr,"%u %u ",(uint)rTimes[i].getSeconds(), (uint)rTimes[i].getUSeconds());
		TPositionLogLine l((uint)rTimes[i].getSeconds(),
									 (uint)rTimes[i].getUSeconds(),
									 poses[i].x,
									 poses[i].y,
									 poses[i].a,
									 0.0, 0.0, 0.0,0.0,0.0,0.0);
	//	fprintf(stderr,"%u %u\n",l.timestamp_s, l.timestamp_us);
		posWriter.WriteLine(l);
		
	}
	posWriter.CloseFile();
	
	return true;
}